import numpy as np
import math
import matplotlib.pyplot as plt
import random
#利用广义数组完成操作
infinity = float('inf')
# 栅格界面场景定义。。
rows = 50
cols = 50
# 定义栅格地图全域，并初始化空白区域
field = np.ones(rows * cols)
# 起始点和目标点
start = 0
shelf = [2+14*20, 6+14*20, 11+14*20, 16+14*20]#货架位置
goal = shelf[3] # 选择货架号数，规划路径至货架旁边

# 障碍物区域
# 要想自定义障碍物区域，只需要修改obsIndex列表的值即可
obsIndex = []
for x in range(2,14):
    for y in range(2, 24, 5):
        obsIndex.append(y + x * cols)
        obsIndex.append(y + 1 + x * cols)
        #obsIndex.append(y + 2 + x * cols)
    for y in range(26, 50, 5):
        obsIndex.append(y + x * cols)
        obsIndex.append(y + 1 + x * cols)
        #obsIndex.append(y + 2 + x * cols)


for x in range(18,30):
    for y in range(2,24,5):
        obsIndex.append(y+x*cols)
        obsIndex.append(y+1+x*cols)
        # obsIndex.append(y+2+x*cols)
    for y in range(26, 50, 5):
        obsIndex.append(y + x * cols)
        obsIndex.append(y + 1 + x * cols)
        # obsIndex.append(y + 2 + x * cols)

# obsRate = 0.2  # 障碍物出现几率
# obsNum = math.floor(obsRate * rows * cols)
# obsIndex = random.sample(range(0, rows * cols), obsNum)
field[obsIndex] = 2
field[start] = 4
field[goal] = 5


# # 查找当前父节点临近8个节点
# def getNeighborNodes(row, col, closelist, parentnode):
#     row_parentnode = parentnode // col
#     col_parentnode = parentnode % col
#     childNode_neighbor = []
#     closelist = closelist[:, 0].tolist()
#     # 第一个节点
#     childnode = [row_parentnode, col_parentnode + 1]
#     if not (childnode[0] < 0 or childnode[0] > row - 1 or childnode[1] < 0 or childnode[1] > col - 1):
#         if field[childnode[0] * col + childnode[1]] != 2:
#             childnode_l = childnode[0] * col + childnode[1]
#             if childnode_l not in closelist:
#                 childNode_neighbor.append(childnode_l)
#     # 第2个节点
#     childnode = [row_parentnode - 1, col_parentnode + 1]
#     if not (childnode[0] < 0 or childnode[0] > row - 1 or childnode[1] < 0 or childnode[1] > col - 1):
#         if field[childnode[0] * col + childnode[1]] != 2:
#             childnode_l = childnode[0] * col + childnode[1]
#             if childnode_l not in closelist:
#                 childNode_neighbor.append(childnode_l)
#     # 第3个节点
#     childnode = [row_parentnode - 1, col_parentnode]
#     if not (childnode[0] < 0 or childnode[0] > row - 1 or childnode[1] < 0 or childnode[1] > col - 1):
#         if field[childnode[0] * col + childnode[1]] != 2:
#             childnode_l = childnode[0] * col + childnode[1]
#             if childnode_l not in closelist:
#                 childNode_neighbor.append(childnode_l)
#     # 第4个节点
#     childnode = [row_parentnode - 1, col_parentnode - 1]
#     if not (childnode[0] < 0 or childnode[0] > row - 1 or childnode[1] < 0 or childnode[1] > col - 1):
#         if field[childnode[0] * col + childnode[1]] != 2:
#             childnode_l = childnode[0] * col + childnode[1]
#             if childnode_l not in closelist:
#                 childNode_neighbor.append(childnode_l)
#     # 第5个节点
#     childnode = [row_parentnode, col_parentnode - 1]
#     if not (childnode[0] < 0 or childnode[0] > row - 1 or childnode[1] < 0 or childnode[1] > col - 1):
#         if field[childnode[0] * col + childnode[1]] != 2:
#             childnode_l = childnode[0] * col + childnode[1]
#             if childnode_l not in closelist:
#                 childNode_neighbor.append(childnode_l)
#     # 第6个节点
#     childnode = [row_parentnode + 1, col_parentnode - 1]
#     if not (childnode[0] < 0 or childnode[0] > row - 1 or childnode[1] < 0 or childnode[1] > col - 1):
#         if field[childnode[0] * col + childnode[1]] != 2:
#             childnode_l = childnode[0] * col + childnode[1]
#             if childnode_l not in closelist:
#                 childNode_neighbor.append(childnode_l)
#     # 第7个节点
#     childnode = [row_parentnode + 1, col_parentnode]
#     if not (childnode[0] < 0 or childnode[0] > row - 1 or childnode[1] < 0 or childnode[1] > col - 1):
#         if field[childnode[0] * col + childnode[1]] != 2:
#             childnode_l = childnode[0] * col + childnode[1]
#             if childnode_l not in closelist:
#                 childNode_neighbor.append(childnode_l)
#     # 第8个节点
#     childnode = [row_parentnode + 1, col_parentnode + 1]
#     if not (childnode[0] < 0 or childnode[0] > row - 1 or childnode[1] < 0 or childnode[1] > col - 1):
#         if field[childnode[0] * col + childnode[1]] != 2:
#             childnode_l = childnode[0] * col + childnode[1]
#             if childnode_l not in closelist:
#                 childNode_neighbor.append(childnode_l)
#     return childNode_neighbor
#
#
# # 初始化closelist
# parentNode = start
# CloseList = np.array([[start, 0]])
#
#
# # 初始化openlist
# class OL:
#     def __init__(self):
#         self.node = 0
#         self.g = 0
#         self.h = 0
#         self.f = 0
#
#
# Openlist = []
# childNodes = getNeighborNodes(rows, cols, CloseList, parentNode)
# for i in range(len(childNodes)):
#     row_start = start // cols
#     col_start = start % cols
#     row_goal = goal // cols
#     col_goal = goal % cols
#     row = childNodes[i] // cols
#     col = childNodes[i] % cols
#     OL_node = OL()
#     OL_node.node = childNodes[i]
#     OL_node.g = np.linalg.norm(np.array([row_start, col_start] - np.array([row, col])))
#     OL_node.h = abs(row_goal - row) + abs(col_goal - col)
#     OL_node.f = OL_node.g + OL_node.h
#     Openlist.append(OL_node)
#
# # 初始化路径
# path = {}
# Openlist_node_f = []
# for i in range(rows * cols):
#     path[i] = []
# for i in range(len(Openlist)):
#     node = Openlist[i].node
#     path[node] = [start, node]
#     Openlist_node_f.append(Openlist[i].f)
#
# # 开始搜索
# # 从OpenList开始搜索移动代价最小的节点
# idx_min = Openlist_node_f.index(min(Openlist_node_f))
# parentNode = Openlist[idx_min].node
#
# # 进入循环
# while True:
#     Openlist_node_node = []
#     Openlist_node_f = []
#     childNodes = getNeighborNodes(rows, cols, CloseList, parentNode)
#
#     for val in Openlist:
#         Openlist_node_node.append(val.node)
#
#     # 判断这些子节点是否在openList中，若在，则比较更新；没在则追加到openList中
#     for i in range(len(childNodes)):
#         childNode = childNodes[i]
#         if childNode in Openlist_node_node:
#             in_flag = 1
#             idx = Openlist_node_node.index(childNode)
#         else:
#             in_flag = 0
#         # 计算代价函数
#         row_parentNode = parentNode // cols
#         col_parentNode = parentNode % cols
#         row_childNode = childNode // cols
#         col_childNode = childNode % cols
#         row_goal = goal // cols
#         col_goal = goal % cols
#         g = Openlist[idx_min].g + np.linalg.norm(
#             np.array([row_parentNode, col_parentNode]) - np.array([row_childNode, col_childNode]))
#         h = abs(row_goal - row_childNode) + abs(col_goal - col_childNode)
#         f = g + h
#         if in_flag:
#             if f < Openlist[idx].f:
#                 Openlist[idx].g = g
#                 Openlist[idx].h=h
#                 Openlist[idx].f=f
#                 path[childNode]=path[parentNode]+[childNode]
#         else:
#             OL_node2 = OL()
#             OL_node2.node=childNode
#             OL_node2.g=g
#             OL_node2.h=h
#             OL_node2.f=f
#             Openlist.append(OL_node2)
#             path[childNode]=path[parentNode]+[childNode]
#
#     # 从openList移出移动代价最小的节点到closeList
#     CloseList=np.vstack([CloseList, [Openlist[idx_min].node, Openlist[idx_min].f]])
#     Openlist.remove(Openlist[idx_min])
#     # 重新搜索
#     for val in Openlist:
#         Openlist_node_f.append(val.f)
#     idx_min = Openlist_node_f.index(min(Openlist_node_f))
#     parentNode=Openlist[idx_min].node
#
#     # 判断是否到达终点
#     if parentNode==goal:
#         CloseList = np.vstack([CloseList, [Openlist[idx_min].node, Openlist[idx_min].f]])
#         break

# path_target=path[goal] # 这是用来显示路径的，暂时不用

# 画图
plt.figure(figsize=(cols, rows))
plt.xlim(-1, cols)
plt.ylim(-1, rows)
my_x_ticks = np.arange(0, cols, 1)  # 规定坐标轴步长
my_y_ticks = np.arange(0, rows, 1)
plt.xticks(my_x_ticks)
plt.yticks(my_y_ticks)
plt.grid(color='k', linestyle='-', linewidth=0.5)  # 开启栅格

ax = plt.gca()  # 获取到当前坐标轴信息
ax.xaxis.set_ticks_position('top')  # 将X坐标轴移到上面
ax.invert_yaxis()  # 反转Y坐标轴

#显示图像的部分
plt.scatter(np.array(obsIndex) % cols, np.array(obsIndex) // cols, s=2000, c='r', marker='s')
#plt.scatter(np.array(path_target) % cols, np.array(path_target) // cols, s=2000, c='y', marker='s') #用来显示路径的，暂时不用
plt.scatter(start % cols, start // cols, s=2000, c='b', marker='s')
plt.scatter(goal % cols, goal // cols, s=2000, c='g', marker='s')
plt.show()